#!/usr/bin/env python3
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import math

class ExplicitStaticTFPublisher:
    def __init__(self):
        # 初始化节点
        rospy.init_node('explicit_static_tf_publisher', anonymous=True)
        
        # 读取参数（默认值为你的相机参数，可通过launch文件修改）
        self.parent_frame = rospy.get_param('~parent_frame', 'grasp_point')
        self.child_frame = rospy.get_param('~child_frame', 'camera_link')
        
        # 平移参数（单位：米）
        self.x = rospy.get_param('~x', 0.016203304511071055)
        self.y = rospy.get_param('~y', 0.028402633714214608)
        self.z = rospy.get_param('~z', -0.0053984118041707)
        
        # 旋转参数（单位：弧度，明确指定roll→pitch→yaw顺序）
        self.roll = rospy.get_param('~roll', 0)   # 绕X轴旋转
        self.pitch = rospy.get_param('~pitch', -1.483)  # 绕Y轴旋转
        self.yaw = rospy.get_param('~yaw', -1.57)      # 绕Z轴旋转
        
        # 静态TF发布器
        self.static_broadcaster = tf2_ros.StaticTransformBroadcaster()
        
        # 构建变换消息
        self.transform = TransformStamped()
        self.transform.header.frame_id = self.parent_frame
        self.transform.child_frame_id = self.child_frame
        
        # 设置平移
        self.transform.transform.translation.x = self.x
        self.transform.transform.translation.y = self.y
        self.transform.transform.translation.z = self.z
        
        # 计算旋转四元数（明确使用roll→pitch→yaw顺序，即xyz欧拉角）
        self.update_rotation()
        
        # 发布静态TF
        self.static_broadcaster.sendTransform(self.transform)
        rospy.loginfo(f"已发布静态TF：{self.parent_frame} → {self.child_frame}")
        rospy.loginfo(f"平移：x={self.x}, y={self.y}, z={self.z}")
        rospy.loginfo(f"旋转（roll→pitch→yaw）：{self.roll:.3f}, {self.pitch:.3f}, {self.yaw:.3f}")

    def update_rotation(self):
        """将roll→pitch→yaw（xyz）欧拉角转换为四元数"""
        # 计算余弦和正弦值
        cr = math.cos(self.roll * 0.5)
        sr = math.sin(self.roll * 0.5)
        cp = math.cos(self.pitch * 0.5)
        sp = math.sin(self.pitch * 0.5)
        cy = math.cos(self.yaw * 0.5)
        sy = math.sin(self.yaw * 0.5)
        
        # 四元数计算公式（xyz顺序）
        qx = sr * cp * cy - cr * sp * sy
        qy = cr * sp * cy + sr * cp * sy
        qz = cr * cp * sy - sr * sp * cy
        qw = cr * cp * cy + sr * sp * sy
        
        self.transform.transform.rotation.x = qx
        self.transform.transform.rotation.y = qy
        self.transform.transform.rotation.z = qz
        self.transform.transform.rotation.w = qw

if __name__ == '__main__':
    try:
        # 节点启动后持续运行（静态TF只需发布一次）
        publisher = ExplicitStaticTFPublisher()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("节点被中断，退出...")
